
#include "ubt_mqtt/mqtt_client.h"

namespace ubt {

static const std::string g_id_     = "amr";
static const std::string admin_    = "admin";
static const std::string password_ = "password";
static const int64_t g_recv_message_timeout = 10 * 1000; // 10s

CMqttClient::CMqttClient()
: mosquittopp(g_id_.c_str())
{
    _message_callback = nullptr;
    _connect_status = false;

    _last_message_recv_time = ubt_utils::getTimeMilliseconds();
}

CMqttClient::~CMqttClient()
{

}

void CMqttClient::on_connect(int rc)
{
    RCLCPP_INFO_STREAM(node->get_logger(),"Connected with code " << rc);

	if (rc == 0)
	{
        std::string subscribe_topic = "agv/linten/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/#";
        RCLCPP_INFO_STREAM(node->get_logger(),"subscribe topic: " << subscribe_topic);
		subscribe(NULL, subscribe_topic.c_str());
        //subscribe(NULL, "agv/linten/#");
        _connect_status = true;
	}
}

void CMqttClient::on_disconnect(int rc)
{
    RCLCPP_INFO_STREAM(node->get_logger(),"mqtt disconnect " << rc);
    _connect_status = false;
}

void CMqttClient::on_message(const struct mosquitto_message *message)
{
    // RCLCPP_INFO_STREAM(node->get_logger(),"on_message-> " << message->topic << ": " << std::string((char*)message->payload));
    // fwrite(message->payload, 1, message->payloadlen, stdout);
    if (_msg_callback_map.find(message->topic) != _msg_callback_map.end())
    {
        _msg_callback_map[message->topic]((char*)message->payload, message->payloadlen);
    }

    _last_message_recv_time = ubt_utils::getTimeMilliseconds();
}

void CMqttClient::on_subscribe(int mid, int qos_count, const int *granted_qos)
{

}

void CMqttClient::on_log(int level, const char *str)
{
    // RCLCPP_INFO_STREAM(node->get_logger(),"level: " << level << ", msg: " << str);
}

void CMqttClient::Connect(const std::string &addr, int port, int keepalive)
{
    address_ = addr;

    username_pw_set(admin_.c_str(), password_.c_str());
    connect(address_.c_str(), port, keepalive);
}

bool CMqttClient::Publish(const char *topic, int payloadlen, const void *payload, int qos, bool retain)
{
    if (_connect_status == true)
    {
        int ret = publish(nullptr, topic, payloadlen, payload, qos, retain);
        if (ret == MOSQ_ERR_SUCCESS)
        {
            return true;
        }
        else
        {
            RCLCPP_ERROR_STREAM(node->get_logger(),"mqtt publish error, topic: " << topic << ", error data: " << ret);
        }
    }

    return false;
}

bool CMqttClient::MqttConnectStatus()
{
    int64_t current_time = ubt_utils::getTimeMilliseconds();

    if (!_connect_status)
    {
        return false;
    }

    // 10s 没有接收到服务器发过来的数据 (会有1s一次的心跳包)
    if (current_time - _last_message_recv_time > g_recv_message_timeout)
    {
        RCLCPP_INFO_STREAM(node->get_logger(),"last recv time: " << _last_message_recv_time);
        RCLCPP_INFO_STREAM(node->get_logger(),"current time: " << current_time);
        return false;
    }

    return true;
}

void CMqttClient::message_callback_add(const std::string &topic, onMessageCallback callback)
{
    RCLCPP_INFO_STREAM(node->get_logger(),"add topic: " << topic);
    _msg_callback_map[topic] = callback;
}

}

